/**
 * PWPathFinderRobot.cpp
 * Copyright : Ms-PL
 * Author : Hiroshi Ota
 */

#include "PWPathFinderRobot.h"

#ifdef _BRICKOS_
#include "c++/Battery.H"
#include "c++/Lamp.h"
#include "c++/LightSensor.h"
#include "c++/TouchSensor.H"
#include "c++/Motor.h"
#include "conio.h"
#include "c++/sound.h"
#include "dkey.h"
#include "time.h"
#include "Key.h"

static Lamp* rcxLamp = 0;
static LightSensor* rcxLightSensor = 0;
static TouchSensor* rcxTouchSensor = 0;
static Motor* rcxDriveMotor = 0;
static Motor* rcxSteeringMotor = 0;
static Key* rcxKey = 0;

#else
#endif

static IKeyHandler* keyHandler;
static IMessageHandler* messageHandler;

PWPathFinderRobot* PWPathFinderRobot::create()
{
	PWPathFinderRobot* instance=0;
#ifdef _BRICKOS_
#else	// _BRICKOS_
#endif	// _BRICKOS_

	return instance;
}

PWPathFinderRobot::PWPathFinderRobot()
{
#ifdef _BRICKOS_
	rcxKey = new Key();
#else	// _BRICKOS_
#endif	// _BRICKOS_
}

PWPathFinderRobot::~PWPathFinderRobot()
{
#ifdef _BRICKOS_
#else	// _BRICKOS_
#endif	// _BRICKOS_
}

int PWPathFinderRobot::batteryLevel()
{
	int value = 0;

#ifdef _BRICKOS_
	value = Battery::get();
#else	// _BRICKOS_
	value = this->simCon->getBatteryValue();
#endif	// _BRICKOS_
	
	return value;
}

void PWPathFinderRobot::light(bool on)
{
#ifdef _BRICKOS_
	rcxLamp->on();
#else	// _BRICKOS_
	this->simCon->setLightStatus(this->lightSensorPort, on);
#endif	// _BRICKOS_
}

void PWPathFinderRobot::lightBrightness(unsigned char level)
{
#ifdef _BRICKOS_
	rcxLamp->brightness(level);
#else	// _BRICKOS_
	this->simCon->setLightBrightness(this->lightSensorPort, level);
#endif	// _BRICKOS_
}

void PWPathFinderRobot::lightSensorDefine(PWPathFinderRobot::SensorPort port)
{
#ifdef _BRICKOS_
	// should be exchange!!
	rcxLamp = new Lamp(port);
	rcxLightSensor = new LightSensor(port);
#else	// _BRICKOS_
	this->lightSensorPort = port;
#endif	// _BRICKOS_
}

unsigned int PWPathFinderRobot::lightSensorGet()
{
	unsigned int value = 0;
#ifdef _BRICKOS_
	value = rcxLightSensor->get();
#else	// _BRICKOS_
	value = this->simCon->getSensorValue(this->lightSensorPort);
#endif	// _BRICKOS_

	return value;
}

void PWPathFinderRobot::lightSensorMode(bool makeActive)
{
#ifdef _BRICKOS_
	rcxLightSensor->mode(makeActive);
#else	// _BRICKOS_
	this->simCon->setSensorMode(this->lightSensorPort,makeActive);
#endif	// _BRICKOS_
}

void PWPathFinderRobot::lcdDraw(char *message)
{
#ifdef _BRICKOS_
	::cputs(message);
#else	// _BRICKOS_
	this->simCon->cputsLCD(message);
#endif	// _BRICKOS_
}

void PWPathFinderRobot::lcdDraw(char mask, int position)
{
#ifdef _BRICKOS_
	::cputc_native(mask, position);
#else	// _BRICKOS_
	this->simCon->cputcLCD(ISimulatorControl::LCD_DISPLAY_MODE_NATIVE, mask, position);
#endif	// _BRICKOS_
}

void PWPathFinderRobot::lcdClear()
{
#ifdef _BRICKOS_
	::cls();
#else	// _BRICKOS_
	this->simCon->clearLCD();
#endif	// _BRICKOS_
}

int PWPathFinderRobot::messageSend(unsigned char *message, unsigned int length)
{
	int result = -1;
#ifdef _BRICKOS_
#else	// _BRICKOS_
	this->simCon->sendMessage(message,length);
#endif	// _BRICKOS_

	return result;
}

int PWPathFinderRobot::messageReceive(unsigned char *&message, unsigned int length, bool blocking)
{
	int result = -1;
#ifdef _BRICKOS_
#else	// _BRICKOS_
	this->simCon->receiveMessage(blocking);
#endif	// _BRICKOS_

	return result;
}

void PWPathFinderRobot::registMessageHandler(IMessageHandler *handler)
{
	messageHandler = handler;
#ifdef _BRICKOS_
#else	// _BRICKOS_
	this->simCon->registMessageDelegator(PWPathFinderRobot::messageWakeup);
#endif	// _BRICKOS_
}

unsigned long PWPathFinderRobot::messageWakeup(void* param)
{
	unsigned char* msg = (unsigned char*)param;
	unsigned int length=0;
	while (*(msg+length)!='\0'){
		length++;
	}
	return messageHandler->received(msg,length);
}

void PWPathFinderRobot::motorDefine(PWPathFinderRobot::MotorType motorType, PWPathFinderRobot::MotorPort port)
{
#ifdef _BRICKOS_
	if (motorType == PWPathFinderRobot::MOTOR_TYPE_DRIVE) {
		rcxDriveMotor = new Motor(port);
	} else {
		rcxSteeringDrive = new Motor(port);
	}
#else	// _BRICKOS_
	if (PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		this->driveMotorPort = port;
	} else {
		this->steeringMotorPort = port;
	}
#endif	// _BRICKOS_
}

void PWPathFinderRobot::motorSpeed(PWPathFinderRobot::MotorType motorType, unsigned char speed)
{
#ifdef _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		rcxDriveMotor->speed(speed);
	} else {
		rcxSteeringDrive->speed(speed);
	}
#else	// _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_DRIVE, "speed",speed);
	} else {
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_STEERING,"speed",speed);
	}
#endif	// _BRICKOS_
}

void PWPathFinderRobot::motorDirection(PWPathFinderRobot::MotorType motorType, PWPathFinderRobot::MotorDirection dir)
{
#ifdef _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		rcxDriveMotor->direction(dir);
	} else {
		rcxSteeringDrive->direction(dir);
	}
#else	// _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_DRIVE, "direction",dir);
	} else {
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_STEERING,"direction",dir);
	}
#endif	// _BRICKOS_
}

void PWPathFinderRobot::motorForward(PWPathFinderRobot::MotorType motorType)
{
#ifdef _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		rcxDriveMotor->forward();
	} else {
		rcxSteeringDrive->forward();
	}
#else	// _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_DRIVE, "forward");
	} else {
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_STEERING,"forward");
	}
#endif	// _BRICKOS_
}

void PWPathFinderRobot::motorReverse(PWPathFinderRobot::MotorType motorType)
{
#ifdef _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		rcxDriveMotor->reverse();
	} else {
		rcxSteeringDrive->reverse();
	}
#else	// _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_DRIVE, "reverse");
	} else {
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_STEERING,"reverse");
	}
#endif	// _BRICKOS_
}

void PWPathFinderRobot::motorOff(PWPathFinderRobot::MotorType motorType)
{
#ifdef _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		rcxDriveMotor->off();
	} else {
		rcxSteeringDrive->off();
	}
#else	// _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_DRIVE, "off");
	} else {
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_STEERING,"off");
	}
#endif	// _BRICKOS_
}

void PWPathFinderRobot::motorBrake(PWPathFinderRobot::MotorType motorType)
{
#ifdef _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		rcxDriveMotor->brake();
	} else {
		rcxSteeringDrive->brake();
	}
#else	// _BRICKOS_
	if (motorType==PWPathFinderRobot::MOTOR_TYPE_DRIVE){
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_DRIVE, "brake");
	} else {
		this->simCon->orderMotor(PWPathFinderRobot::MOTOR_TYPE_STEERING,"brake");
	}
#endif	// _BRICKOS_
}

void PWPathFinderRobot::touchSensorDefine(PWPathFinderRobot::SensorPort port)
{
#ifdef _BRICKOS_
	// should be!!
	rcxTouchSensor = new TouchSensor(port);
#else	// _BRICKOS_
	this->touchSensorPort = port;
#endif	// _BRICKOS_
}

void PWPathFinderRobot::touchSensorMode(bool makeActive)
{
#ifdef _BRICKOS_
	rcxTouchSensor->mode(makeActive);
#else	// _BRICKOS_
	this->simCon->setSensorMode(this->touchSensorPort, makeActive);
#endif	// _BRICKOS_
}

bool PWPathFinderRobot::touchSensorTouched()
{
	bool value = false;
#ifdef _BRICKOS_
	value = rcxTouchSensor->touched();
#else	// _BRICKOS_
	value = (this->simCon->getSensorValue(this->touchSensorPort)!=0);
#endif	// _BRICKOS_

	return value;
}

void PWPathFinderRobot::registKeyHandler(IKeyHandler *handler)
{
	keyHandler = handler;
#ifdef _BRICKOS_
	rcxKey->taskStart(PWPathFinderRobot::keyWakeup);
#else	// _BRICKOS_
	this->simCon->registKeyDelegator(PWPathFinderRobot::keyWakeup);
#endif	// _BRICKOS_
}

PWPathFinderRobot::KeyType PWPathFinderRobot::keyPressed(PWPathFinderRobot::KeyType keyType)
{
        KeyType key = (KeyType)KEY_ANY;
#ifdef _BRICKOS_
        key = (enum KEY)dkey_pressed(keys);
#else
		if (simCon!=0) {
			key = (KeyType)simCon->getKeyStatus((int)keyType,true);
		}
#endif	// _BRICKOS_
		return key;
}

PWPathFinderRobot::KeyType PWPathFinderRobot::keyReleased(PWPathFinderRobot::KeyType keys)
{
        KeyType key = (KeyType)KEY_ANY;
#ifdef _BRICKOS_
        key = (enum KEY)dkey_pressed(keys);
#else
		if (simCon!=0) {
			key = (KeyType)simCon->getKeyStatus((int)keys,true);
		}
#endif	// _BRICKOS_
		return key;
}

unsigned long PWPathFinderRobot::keyWakeup(void *param)
{
	if (keyHandler != 0){
		keyHandler->pushed((unsigned char)param);
	}
	return 0;
}

void PWPathFinderRobot::soundBeep()
{
#ifdef _BRICKOS_
	Sound::beep();
#else	// _BRICKOS_
	this->simCon->beepSound();
#endif	// _BRICKOS_
}

void PWPathFinderRobot::soundDuration(unsigned int length)
{
#ifdef _BRICKOS_
	Sound::duration(length);
#else	// _BRICKOS_
	this->simCon->setSoundDuration(length);
#endif	// _BRICKOS_
}

void PWPathFinderRobot::soundInternote(unsigned int length)
{
#ifdef _BRICKOS_
	Sound::internote(length);
#else	// _BRICKOS_
	this->simCon->setSoundInternote(length);
#endif	// _BRICKOS_
}

void PWPathFinderRobot::soundPlay(SoundNote *note, unsigned int length)
{
#ifdef _BRICKOS_
	note_t bNotes;
	bNotes.pitch = notes->pitch;
	bNotes.length = notes->length;
	Sound::play(&bNotes);
#else	// _BRICKOS_
	this->simCon->play(note->pitch, note->duration);
#endif	// _BRICKOS_
}

int PWPathFinderRobot::soundPlaying()
{
	int result = 0;
#ifdef _BRICKOS_
	result = Sound::playing();
#else	// _BRICKOS_
	//
#endif	// _BRICKOS_

	return result;
}

void PWPathFinderRobot::soundStop()
{
#ifdef _BRICKOS_
#else	// _BRICKOS_
#endif	// _BRICKOS_
}

PWPathFinderRobot::time_t PWPathFinderRobot::timeGetSystemUp()
{
	PWPathFinderRobot::time_t value = 0;

#ifdef _BRICKOS_
	valueget_system_up_time();
#else	// _BRICKOS_
#endif	// _BRICKOS_
	return value;
}

int PWPathFinderRobot::storageWrite(PWPathFinderRobot::time_t tick, unsigned char *buf, unsigned int length)
{
	int result = 0;
#ifdef _BRICKOS_
#else	// _BRICKOS_
#endif	// _BRICKOS_

	return result;
}

int PWPathFinderRobot::storageRead(PWPathFinderRobot::time_t &tick,unsigned char *&buf, unsigned int &length, bool fromFirst)
{
	int result = 0;
#ifdef _BRICKOS_
#else	// _BRICKOS_
#endif	// _BRICKOS_

	return result;
}

void PWPathFinderRobot::storageClear()
{
#ifdef _BRICKOS_
#else	// _BRICKOS_
#endif	// _BRICKOS_
}


#ifndef _BRICKOS_
void PWPathFinderRobot::Initialize(ISimulatorControl *sc)
{
	this->simCon = sc;
}

void PWPathFinderRobot::Terminate()
{
	;
}

#endif // __BRICKOS_
